#include <iostream>
#include <string>
#include <stdio.h>
#include <fstream>
#include <ctime>
#include <thread>
#include <mutex>
#include <fstream>
#include <opencv2/opencv.hpp>

#include "acl/acl.h"
#include "atlas_model.h"
#include "string.h"
#include "unistd.h"
#include "dimp/dimp_tracker.h"
#include "dimp/preprocess_for_init.hpp"
#include "dimp/dimp_init.hpp"
#include "dimp/dimp_tracking.hpp"
#include "thread_safe_queue.h"


using namespace std;
using namespace cv;

std::mutex mtx;

Tracker::Tracker(const char* modelpath[], string filepath)
:deviceId_(0), context_(nullptr), stream_(nullptr), isInited_(false),filepath_(filepath),
Image_pre_(filepath),
Dimp_init_(modelpath[0]),
Dimp_tracking_(modelpath[1], modelpath[2]),
DiMPSteepestDescendGN_(modelpath[3], modelpath[4])
{}

Tracker::~Tracker() {
    DestroyResource();
}

AtlasError Tracker::InitResource() {
    // ACL init
    const char *aclConfigPath = "../src/acl.json";
    aclError ret = aclInit(aclConfigPath);
    if (ret != ACL_ERROR_NONE) {
        ERROR_LOG("acl init failed");
        return FAILED;
    }
    INFO_LOG("acl init success");

    // open device
    ret = aclrtSetDevice(deviceId_);
    if (ret != ACL_ERROR_NONE) {
        ERROR_LOG("acl open device %d failed", deviceId_);
        return FAILED;
    }
    INFO_LOG("open device %d success", deviceId_);

    // create context (set current)
    ret = aclrtCreateContext(&context_, deviceId_);
    if (ret != ACL_ERROR_NONE) {
        ERROR_LOG("acl create context failed");
        return FAILED;
    }
    INFO_LOG("create context success");

    // create stream1
    ret = aclrtCreateStream(&stream_);
    if (ret != ACL_ERROR_NONE) {
        ERROR_LOG("acl create stream failed");
        return FAILED;
    }
    INFO_LOG("create stream success");

    ret = aclrtGetRunMode(&runMode_);
    if (ret != ACL_ERROR_NONE) {
        ERROR_LOG("acl get run mode failed");
        return FAILED;
    }

    // ret = aclrtCreateEvent(&update_done_);
    // if (ret != ACL_ERROR_NONE) {
    //     ERROR_LOG("acl create event failed");
    //     return FAILED;
    // }
    // INFO_LOG("create event success");

    
    return ATLAS_OK;
}

AtlasError Tracker::LoadAllModel(int32_t& deviceId, aclrtContext& context, aclrtStream& stream, aclrtRunMode& runMode) {
    deviceId_ = deviceId;
    context_ = context;
    stream_ = stream;
    runMode_ = runMode;

    aclError ret = Dimp_init_.Init(deviceId_, context_, stream_, runMode_);
    if (ret != ATLAS_OK) {
        ATLAS_LOG_ERROR("Dimp Initial failed!");
        return ret;
    }

    ret = Dimp_tracking_.Init(deviceId_, context_, stream_, runMode_);
    if (ret != ATLAS_OK) {
        ATLAS_LOG_ERROR("Dimp tracking model initial failed!");
        return ret;
    }

    ret = DiMPSteepestDescendGN_.Init(deviceId_, context_, stream_, runMode_);
    if (ret != ATLAS_OK) {
        ATLAS_LOG_ERROR("Dimp initial optimizer model initial failed!");
        return ret;
    }

    return ATLAS_OK;
}
AtlasError Tracker::InitForAll() {
    AtlasError ret = InitResource();
    if (ret != ATLAS_OK) {
        ATLAS_LOG_ERROR("Init acl resource failed, error: %d", ret);
        return ret;
    }
    return SUCCESS;
}

AtlasError Tracker::InitFirstFrame(cv::Mat img) {
    if (isInited_) {
        ATLAS_LOG_INFO("instance is initied already!\n");
        return ATLAS_OK;
    }

    AtlasError ret = Image_pre_.Init(transform_imgs_, transform_boxes_, init_base_target_size_, init_sample_pos_, init_target_scale_, init_target_size_, init_target_pos_, img);
    if (ret != ATLAS_OK) {
        ATLAS_LOG_ERROR("Image Preprocess failed!");
        return ret;
    }


    ret = Dimp_init_.LoadInputData(transform_imgs_, transform_boxes_);
    if (ret != ATLAS_OK) {
        ATLAS_LOG_ERROR("Dimp Initial failed!");
        return ret;
    }


    isInited_ = true;

    ret = Dimp_init_.Inference();
    if (ret != ATLAS_OK) {
        ATLAS_LOG_ERROR("Dimp Initial infernce failed!");
        return ret;
    }
    DiMPSteepestDescendGN_.LoadInputDataForInitialOptimizer(Dimp_init_.classifier_feats_, Dimp_init_.weights_, transform_boxes_);
    DiMPSteepestDescendGN_.InferenceForInitialOptimizer();

    origin_weight_ = (void*)DiMPSteepestDescendGN_.final_weights_.firstaddr;

    if (ret != ATLAS_OK) {
        ATLAS_LOG_ERROR("Dimp Initial optimizer infernce failed!");
        return ret;
    }

    thread update(&Tracker::Update, this, context_);
    update.detach();

    sample_pos_.x = init_sample_pos_.x;
    sample_pos_.y = init_sample_pos_.y;
    target_scale_ = init_target_scale_;
    target_size_.x = init_target_size_.x;
    target_size_.y = init_target_size_.y;
    target_pos_.x = init_target_pos_.x;
    target_pos_.y = init_target_pos_.y;
    frame_nums_ = 1;
    index_ = 0;
    train_nums_ = 1;

    return ATLAS_OK;
}

AtlasError Tracker::Tracking(cv::Mat img, Target_Box& result){
    struct timeval begin;
    struct timeval end;

    struct timeval begin2;
    struct timeval end2;
   
    image_size_.x = img.rows;
    image_size_.y = img.cols;
   
    new_sample_size_.x = 288 * target_scale_;
    new_sample_size_.y = 288 * target_scale_;
 

    sample_image_ = Image_pre_.sample_patch(img, sample_pos_, new_sample_size_, { 288, 288 }, &sample_coords_);
   
    /*****计算sample_pos,对应get_sample_location()函数*****/
    sample_pos_.x = (sample_coords_.top_left.x + sample_coords_.bottom_right.x - 1) * 0.5;
    sample_pos_.y = (sample_coords_.top_left.y + sample_coords_.bottom_right.y - 1) * 0.5;
    sample_scales_ = sqrt((sample_coords_.bottom_right.x - sample_coords_.top_left.x) * (sample_coords_.bottom_right.y - sample_coords_.top_left.y) / (288 * 288));
    
    Dimp_tracking_.LoadInputDataForScoreModel(sample_image_, origin_weight_);

    Dimp_tracking_.InferenceForScores();
    
    now_state_ = localize(Dimp_tracking_.scores_, sample_pos_, sample_scales_, target_size_);
    new_target_pos_.x = sample_pos_.x + now_state_.translation_vec.x;
    new_target_pos_.y = sample_pos_.y + now_state_.translation_vec.y;
    
    if (new_target_pos_.x < 0) {
        new_target_pos_.x = 1;
    }
    if (new_target_pos_.y < 0) {
        new_target_pos_.y = 1;
    }
    if (new_target_pos_.x > image_size_.x) {
        new_target_pos_.x = image_size_.x - 1;
    }
    if (new_target_pos_.y > image_size_.y) {
        new_target_pos_.y = image_size_.y - 1;
    }

    if (now_state_.state != NOT_FOUND) {
       
        init_iou_box_ = Image_pre_.get_iou_box(new_target_pos_, target_size_, sample_pos_, sample_scales_);

        AtlasError ret = Dimp_tracking_.LoadInputDataForIouModelOnce(Dimp_init_.modulation1_.firstaddr, Dimp_init_.modulation2_.firstaddr, init_iou_box_);
       

        if (ret != ATLAS_OK) {
            ATLAS_LOG_ERROR("LoadInputData failed!");
        }
        // gettimeofday(&begin, NULL);
        // ret = Dimp_tracking_.InferenceForIou();

        if (ret != ATLAS_OK) {
            ATLAS_LOG_ERROR("IoUModel Inference failed!");
        }
      
        // // Dimp_tracking_.PostProcessForIouModel();
        // cout<<"begin inference_for_iou"<<endl;
        /***************************************************gradient-ascent*************************/
        for (int i = 0; i < 2; ++i) {
            // cout<<"inference + "<< i <<endl;
            ret = Dimp_tracking_.InferenceForIou();   //推理得到iou
            if (ret != ATLAS_OK) {
                ATLAS_LOG_ERROR("InferenceForIou failed!");
            }

            ret = Dimp_tracking_.PostProcessForIouModel();        //依据iou梯度上升优化bboxes
            if (ret != ATLAS_OK) {
                ATLAS_LOG_ERROR("PostProcessForIouModel failed!");
            }

            ret = Dimp_tracking_.LoadInputDataForIouModelAlways();    //将优化后的bboxes重新加入inputdata
            if (ret != ATLAS_OK) {
                ATLAS_LOG_ERROR("LoadInputDataForIouModelAlways failed!");
            }
        }
        /************************************************************************************************/
        // cout<<"inference for iou end"<<endl;
        Dimp_tracking_.GetOutputBox();
        // INFO_LOG("GetOutputBox time: %fms", time_cost);
        // gettimeofday(&end, NULL);
        // time_cost = 1000 * (end.tv_sec - begin.tv_sec) + (end.tv_usec - begin.tv_usec) / 1000.000;
        // INFO_LOG("IoUmodel inference time: %fms", time_cost);
        // average_ioumodel_time_ = (average_ioumodel_time_ * (frame_nums_ - 1) + time_cost) / (frame_nums_);
        // INFO_LOG("Average IoUmodel time: %fms", average_ioumodel_time_);

        new_pos_.x = (Dimp_tracking_.predicted_box_.top_left.y + Dimp_tracking_.predicted_box_.bottom_right.y) / 2.0;
        new_pos_.y = (Dimp_tracking_.predicted_box_.top_left.x + Dimp_tracking_.predicted_box_.bottom_right.x) / 2.0;

        new_pos_.x = (new_pos_.x - (288 - 1) / 2.0) * sample_scales_ + sample_pos_.x;
        new_pos_.y = (new_pos_.y - (288 - 1) / 2.0) * sample_scales_ + sample_pos_.y;

        target_size_.x = (Dimp_tracking_.predicted_box_.bottom_right.y - Dimp_tracking_.predicted_box_.top_left.y) * sample_scales_;
        target_size_.y = (Dimp_tracking_.predicted_box_.bottom_right.x - Dimp_tracking_.predicted_box_.top_left.x) * sample_scales_;

        target_scale_ = sqrt((target_size_.x * target_size_.y) / (init_base_target_size_.x * init_base_target_size_.y));
        // gettimeofday(&end, NULL);
        // time_cost = 1000 * (end.tv_sec - begin.tv_sec) + (end.tv_usec - begin.tv_usec) / 1000.000;
        // INFO_LOG("IouModel inference time: %fms", time_cost);
    }
    // gettimeofday(&end, NULL);
    // time_cost = 1000 * (end.tv_sec - begin.tv_sec) + (end.tv_usec - begin.tv_usec) / 1000.000;
    // INFO_LOG("IoUmodel inference time: %fms", time_cost);
    bool update = false;
    if (frame_nums_ % 20 == 0 || now_state_.state == HARD_NEGATIVE) {
        update = true;
    }

    UPDATE_INFO data = {new_pos_, target_size_, sample_pos_, sample_scales_, Dimp_tracking_.classifier_feat_, origin_weight_ , now_state_.state, update, frame_nums_};

    if (Update_InfoQueue_.Push(data)){
        // cout<<"push data success!"<<endl;
    }

    // gettimeofday(&begin, NULL);
    // if (frame_nums_ < 40) {
    //     iou_box_ = Image_pre_.get_iou_box(new_pos_, target_size_, sample_pos_, sample_scales_);
    //     // cout<<"Compute iou_box successfully!"<<endl；
    //     // cout<<frame_nums<<endl;
    //     DiMPSteepestDescendGN_.LoadInputDataForTrackingOptimizer(Dimp_tracking_.classifier_feat_, origin_weight_ ,train_nums_, frame_nums_, now_state_.state, iou_box_.pos, iou_box_.size);
    //     train_nums_ +=1;
    // }

    // if (frame_nums_ > 40) {
    //     if(now_state_.state == NORMAL || now_state_.state == HARD_NEGATIVE) {
    //         iou_box_ = Image_pre_.get_iou_box(new_pos_, target_size_, sample_pos_, sample_scales_);
    //         // cout<<"Compute iou_box successfully!"<<endl;
    //         // cout<<frame_nums<<endl;
    //         DiMPSteepestDescendGN_.LoadInputDataForTrackingOptimizer(Dimp_tracking_.classifier_feat_, origin_weight_ ,train_nums_, frame_nums_, now_state_.state, iou_box_.pos, iou_box_.size);
    //         train_nums_ +=1;
    //         // cout << "frame_nums = "<< frame_nums<<endl;
        
    //     }
    // }
    // gettimeofday(&end, NULL);
    // time_cost = 1000 * (end.tv_sec - begin.tv_sec) + (end.tv_usec - begin.tv_usec) / 1000.000;
    // INFO_LOG("Load train_data time: %fms", time_cost);

    

    sample_pos_.x = new_pos_.x;
    sample_pos_.y = new_pos_.y;
    result = {{sample_pos_.y - target_size_.y / 2, sample_pos_.x - target_size_.x / 2}, {target_size_.y, target_size_.x}};
    frame_nums_++;

    cout<<"predict bbox = "<< sample_pos_.x - target_size_.x / 2 << "  +   "<<sample_pos_.y - target_size_.y / 2<<"  +  "<<target_size_.x<<"  +  "<<target_size_.y<<endl;
    cout<<"now_state = "<<now_state_.state<<endl;
    cout<<"now_score = "<<now_state_.score<<endl;
    // gettimeofday(&end2, NULL);
    // time_cost = 1000 * (end2.tv_sec - begin2.tv_sec) + (end2.tv_usec - begin2.tv_usec) / 1000.000;
    // *time_use = time_cost;
    // INFO_LOG("One frame infernce time: %fms, state is: %d", time_cost, now_state_.state);
    // int x = int(sample_pos_.x - target_size_.x / 2);
    // int y = int(sample_pos_.y - target_size_.y / 2);
    // int w = int(target_size_.x);
    // int h = int(target_size_.y);
    // fstream f;
    // f.open("result.txt", ios::out|ios::app);
    // f << x << "    " << y << "    " << w << "    " << h << endl;
    // f.close();
    // vector<BBox> trackResults;
    // auto bbox = cv::Rect(y, x, h, w);
    // storage_.draw_trajectories(img, )


    // cv::cvtColor(img, img, COLOR_RGB2BGR);
   
    // if(now_state_.state == 0) {
    //     string str = "normal";
    //     cv::putText(img, str, cv::Point(25, 25), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 255, 0), 2);
    // }

    // if(now_state_.state == 1) {
    //     string str = "not_found";
    //     cv::putText(img, str, cv::Point(25, 25), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 255), 2);
    // }

    // if(now_state_.state == 2) {
    //     string str = "uncertain";
    //     cv::putText(img, str, cv::Point(25, 25), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 255, 0), 2);
    // }

    // if(now_state_.state == 3) {
    //     string str = "hard_negative";
    //     cv::putText(img, str, cv::Point(25, 25), cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 255, 0), 2);
    // }
    
    // char picname[10];
    // sprintf(picname, "%04d", index_);
    // string full_path;
    // string houzhui = ".jpg";
    // full_path = picname + houzhui;
    // cout<<full_path<<endl;
    // index_ += 1;
    // cv::imwrite(full_path,img);
        
    return SUCCESS;
}

AtlasError Tracker::Update(aclrtContext context) {

    aclrtSetCurrentContext(context);
    UPDATE_INFO data;
    struct timeval begin1;
    struct timeval end1;

    
    while(true) {
        
        data = this->Update_InfoQueue_.PopAndWait();
        // cout << "frame = " << data.frame_num << endl;
        if (data.frame_num < 40) {
            iou_box_ = Image_pre_.get_iou_box(data.new_pos, data.target_size, data.sample_pos, data.sample_scales);
            DiMPSteepestDescendGN_.LoadInputDataForTrackingOptimizer(data.classifier_feat, data.origin_weight ,train_nums_, data.frame_num, data.object_state, iou_box_.pos, iou_box_.size);
            train_nums_ +=1;
        }

        if (data.frame_num >= 40) {
            if(now_state_.state == NORMAL || now_state_.state == HARD_NEGATIVE){
                iou_box_ = Image_pre_.get_iou_box(data.new_pos, data.target_size, data.sample_pos, data.sample_scales);
                DiMPSteepestDescendGN_.LoadInputDataForTrackingOptimizer(data.classifier_feat, data.origin_weight ,train_nums_, data.frame_num, data.object_state, iou_box_.pos, iou_box_.size);
                train_nums_ +=1;
            }  
        }
        if (frame_nums_ % 20 == 0 || now_state_.state == HARD_NEGATIVE) {
            // gettimeofday(&begin1, NULL);
            // cout<<"***********************start updating!**************************************"<<endl;
        
            AtlasError ret = DiMPSteepestDescendGN_.InferenceForTrackingOptimizer(true, true);
            if (ret != ATLAS_OK) {
                return FAILED;
            }
            // mtx.lock();
            origin_weight_ = DiMPSteepestDescendGN_.new_weights_.firstaddr;
            // mtx.unlock();
            // cout<<"*****************************update weights successfully!*************************"<<endl;
            // gettimeofday(&end1, NULL);
            // float time_cost = 1000 * (end1.tv_sec - begin1.tv_sec) + (end1.tv_usec - begin1.tv_usec) / 1000.000;
            // cout<< "+++++++thread update time:"<<time_cost<<"+++++++++++++"<<endl;
            // INFO_LOG("+++++++thread update time: %fms+++++++++++++++", time_cost);
        }
        //  if (frame_nums_ % 25 == 0) {
        //     gettimeofday(&begin1, NULL);
        //     cout<<"***********************start updating!**************************************"<<endl;
        
        //     AtlasError ret = DiMPSteepestDescendGN_.InferenceForTrackingOptimizer(true, true);
        //     if (ret != ATLAS_OK) {
        //         return FAILED;
        //     }
        //     // mtx.lock();
        //     origin_weight_ = DiMPSteepestDescendGN_.new_weights_.firstaddr;
        //     // mtx.unlock();
        //     cout<<"*****************************update weights successfully!*************************"<<endl;
        //     gettimeofday(&end1, NULL);
        //     float time_cost = 1000 * (end1.tv_sec - begin1.tv_sec) + (end1.tv_usec - begin1.tv_usec) / 1000.000;
        //     // cout<< "+++++++thread update time:"<<time_cost<<"+++++++++++++"<<endl;
        //     INFO_LOG("+++++++thread update time: %fms+++++++++++++++", time_cost);
        // }
    }


    return SUCCESS;
}

// AtlasError Tracker::DoProcess(cv::Mat img) {
//     cout<<"start"<<endl;
//     thread track(&Tracker::Tracking, this, img, context_);
//     thread update(&Tracker::Update, this, context_);
//     track.join();
//     update.join();
// }


void Tracker::DestroyResource()
{
    
    Dimp_init_.DestroyResource();
    Dimp_tracking_.DestroyResource();
    DiMPSteepestDescendGN_.DestroyResource();

    aclError ret;
    if (stream_ != nullptr) {
        ret = aclrtDestroyStream(stream_);
        if (ret != ACL_ERROR_NONE) {
            ATLAS_LOG_ERROR("destroy stream failed");
        }
        stream_ = nullptr;
    }
   
    INFO_LOG("end to destroy stream");

    if (context_ != nullptr) {
        ret = aclrtDestroyContext(context_);
        if (ret != ACL_ERROR_NONE) {
            ERROR_LOG("destroy context failed");
        }
        context_ = nullptr;
    }
    INFO_LOG("end to destroy context");

    ret = aclrtResetDevice(deviceId_);
    if (ret != ACL_ERROR_NONE) {
        ATLAS_LOG_ERROR("reset device failed\n");
    }
    ATLAS_LOG_INFO("end to reset device is %d\n", deviceId_);

    ret = aclFinalize();
    if (ret != ACL_ERROR_NONE) {
        ATLAS_LOG_ERROR("finalize acl failed\n");
    }
    ATLAS_LOG_INFO("end to finalize acl");

}

struct STATE Tracker::localize(Nodes scores, Vector sample_pos, float sample_scales, Vector target_size) {
    float max_score1, max_score2;
    STATE now_state;
    Vector target_disp1, target_disp2;
    float scores_masked[19][19];
    int index1, index2;
    int col;
    int row;
    float* scores_ptr = (float*)scores.firstaddr;
    float* scores_masked_ptr = scores_masked[0];
    max_score1 = scores_ptr[0];
    // cout<<"target_size = "<<target_size_.x<<", "<<target_size_.y<<endl;
    // cout<<"**********score***************"<<endl;
    index1 = 0;
    index2 = 0;
    for(int i = 0; i < 361; ++i)
    {
        // cout<<i<<" + "<<scores_ptr[i]<<endl;
        if(scores_ptr[i] > max_score1) {
            max_score1 = scores_ptr[i];
            index1 = i;
        }
    }
    // cout<<"index = "<<index1<<endl;
    row = index1 / 19;
    col = index1 % 19;
    target_disp1.x = (row - 9) * (288.0 / 18.0) * sample_scales;
    target_disp1.y = (col - 9) * (288.0 / 18.0) * sample_scales;
    // cout<<"*********score***************"<<endl;
    // cout<<"target_disp.x = "<<target_disp1.x<<endl;
    // cout<<"target_disp.y = "<<target_disp1.y<<endl;
    // cout<<"mas_score = "<<max_score1<<endl;

    if (max_score1 < 0.25) {
        now_state.translation_vec.x = target_disp1.x;
        now_state.translation_vec.y = target_disp1.y;
        now_state.score = max_score1;
        now_state.state = NOT_FOUND;      
        return now_state;
    }

    Vector target_neigh_sz;
    target_neigh_sz.x = 2.2 * (target_size.x / sample_scales) * (18.0 / 288.0);
    target_neigh_sz.y = 2.2 * (target_size.y / sample_scales) * (18.0 / 288.0);

    // cout<<"target_neight_sz = "<<target_neigh_sz.x<<", "<<target_neigh_sz.y<<endl;

    int tneigh_top = max(int(row - target_neigh_sz.x / 2 + 0.5), 0);
    int tneigh_bottom = min(int(row + target_neigh_sz.x / 2 + 1.5), 19);
    int tneigh_left = max(int(col - target_neigh_sz.y / 2 + 0.5), 0);
    int tneigh_right = min(int(col  + target_neigh_sz.y / 2 + 1.5), 19);

    // cout<<"tneigh = "<<tneigh_top<<","<<tneigh_bottom<<","<<tneigh_left<<","<<tneigh_right<<endl;

    max_score2 = -100;
    for(int i = 0; i < 361; ++i) {
        if((i / 19 >= tneigh_top) && (i / 19 < tneigh_bottom) && (i % 19 >= tneigh_left) && (i % 19 < tneigh_right)) {
            scores_masked_ptr[i] = 0;
        }
        else{
            scores_masked_ptr[i] = scores_ptr[i];
        }
        if (scores_masked_ptr[i] > max_score2) {
            max_score2 = scores_masked_ptr[i];
            index2 = i;
        }
        
    }
    row = index2 / 19;
    col = index2 % 19;
    target_disp2.x = (row - 9) * (288 / 18) * sample_scales;
    target_disp2.y = (col - 9) * (288 / 18) * sample_scales;

    if (max_score2 > 0.8 * max_score1) {
        float disp_norm1 = sqrt(target_disp1.x * target_disp1.x + target_disp1.y * target_disp1.y);
        float disp_norm2 = sqrt(target_disp2.x * target_disp2.x + target_disp2.y * target_disp2.y);
        float disp_threshold = 0.8 * 19 / 2;

        if (disp_norm2 > disp_threshold && disp_norm1 < disp_threshold) {
            now_state.translation_vec.x = target_disp1.x;
            now_state.translation_vec.y = target_disp1.y;
            now_state.score = max_score1;
            now_state.state = HARD_NEGATIVE;
            return now_state;
        }

        if (disp_norm2 < disp_threshold && disp_norm1 > disp_threshold) {
            now_state.translation_vec.x = target_disp2.x;
            now_state.translation_vec.y = target_disp2.y;
            now_state.score = max_score2;
            now_state.state = HARD_NEGATIVE;
            return now_state;
        }
        if (disp_norm2 > disp_threshold && disp_norm1 > disp_threshold) {
            now_state.translation_vec.x = target_disp1.x;
            now_state.translation_vec.y = target_disp1.y;
            now_state.score = max_score1;
            now_state.state = UNCERTAIN;
            return now_state;
        }

        now_state.translation_vec.x = target_disp1.x;
        now_state.translation_vec.y = target_disp1.y;
        now_state.score = max_score1;
        now_state.state = UNCERTAIN;
        return now_state;

    }

    if(max_score2 > 0.5 * max_score1 && max_score2 > 0.25) {
        now_state.translation_vec.x = target_disp1.x;
        now_state.translation_vec.y = target_disp1.y;
        now_state.score = max_score1;
        now_state.state = HARD_NEGATIVE;
        return now_state;
    }
    now_state.translation_vec.x = target_disp1.x;
    now_state.translation_vec.y = target_disp1.y;
    now_state.score = max_score1;
    now_state.state = NORMAL;
    return now_state;
}

